#include <cmath>
#include <limits>
#include <sstream>
#include <string>
#include <angles/angles.h>
#include <jaguar/ares_drive.h>
#include <ros/ros.h>

using can::JaguarBridge;

namespace jaguar {

template <typename T>
inline T sgn(T x)
{
    if      (x > 0) return  1;
    else if (x < 0) return -1;
    else            return  0;
}

AresDriveRobot::AresDriveRobot(AresDriveSettings const &settings)
    : bridge_(settings.port)
    , jag_broadcast_(bridge_)
    , jag_1_(bridge_, settings.id_1)
    , jag_2_(bridge_, settings.id_2)
    , jag_3_(bridge_, settings.id_3)
    , jag_4_(bridge_, settings.id_4)
    , diag_init_(false)
    // These are set by dynamic_reconfigure. However, there is a race condition
    // in waiting for the callback. These are sane defaults to prevent
    // generating +/-infinity or NaN during the race.
    , accel_max_(settings.accel_max_mps2)
    , wheel_circum_(0.0), wheel_sep_(0.0)
    , flip_1_((settings.flip_1) ? -1.0 : 1.0)
    , flip_2_((settings.flip_2) ? -1.0 : 1.0)
    , flip_3_((settings.flip_3) ? -1.0 : 1.0)
    , flip_4_((settings.flip_4) ? -1.0 : 1.0)
{
    // This is necessary for the Jaguars to work after a fresh boot, even if
    // we never called system_halt() or system_reset().
    ROS_INFO("Brake settings start");
    block(
        jag_1_.config_brake_set(settings.brake),
        jag_2_.config_brake_set(settings.brake),
        jag_3_.config_brake_set(settings.brake),
        jag_4_.config_brake_set(settings.brake)
    );
    ROS_INFO("Brake settings complete");

    speed_init();
    ROS_INFO("speed_init complete");
    //odom_init();
    //ROS_INFO("odom_init complete");
    //diag_init();
    //ROS_INFO("diag_init complete");
    ROS_INFO("diag_init complete");
    jag_broadcast_.system_resume();
    ROS_INFO("jag_broadcast_.system_resume complete");
}

AresDriveRobot::~AresDriveRobot(void)
{
}

void AresDriveRobot::drive(double v, double omega)
{
    if (wheel_circum_ == 0 || wheel_sep_ == 0) return;
    double const v_1 = v - 0.5 * wheel_sep_ * omega;
    double const v_2 = v + 0.5 * wheel_sep_ * omega;
    double const v_3 = v;
    double const v_4 = v;
    drive_raw(v_1, v_2, v_3, v_4);
    ROS_INFO("v_1: %f, v_2: %f, v_3: %f, v_4: %f", v_1, v_2, v_3, v_4);
}

void AresDriveRobot::drive_raw(double v_1, double v_2, double v_3, double v_4)
{
    if (wheel_circum_ == 0 || wheel_sep_ == 0) return;
    target_rpm_1_ = v_1 * 60 / wheel_circum_;
    target_rpm_2_ = -v_2 * 60 / wheel_circum_;
    target_rpm_3_ = v_3 * 60 / wheel_circum_;
    target_rpm_4_ = -v_4 * 60 / wheel_circum_;
}

void AresDriveRobot::drive_brake(bool braking)
{
    jaguar::BrakeCoastSetting::Enum value;
    if (braking) {
        value = jaguar::BrakeCoastSetting::kOverrideBrake;
    } else {
        value = jaguar::BrakeCoastSetting::kOverrideCoast;
    }

    block(
        jag_1_.config_brake_set(value),
        jag_2_.config_brake_set(value),
        jag_3_.config_brake_set(value),
        jag_4_.config_brake_set(value)
    );
}
void AresDriveRobot::drive_spin(double dt)
{
    //ROS_INFO("drive_spin start");
    if (wheel_circum_ == 0 || wheel_sep_ == 0) return;
    double const residual_rpm_1 = target_rpm_1_ - current_rpm_1_;
    double const residual_rpm_2 = target_rpm_2_ - current_rpm_2_;
    double const residual_rpm_3 = target_rpm_3_ - current_rpm_3_;
    double const residual_rpm_4 = target_rpm_4_ - current_rpm_4_;
    //ROS_INFO("residual rpm's calculated");
    // Cap the acceleration at the limiting value.
    double const drpm_max = accel_max_ * dt * 60 / wheel_circum_;

    if (fabs(residual_rpm_1) <= drpm_max) {
        current_rpm_1_ = target_rpm_1_;
    } else {
        current_rpm_1_ += sgn(residual_rpm_1) * drpm_max;
    }

    if (fabs(residual_rpm_2) <= drpm_max) {
        current_rpm_2_ = target_rpm_2_;
    } else {
        current_rpm_2_ += sgn(residual_rpm_2) * drpm_max;
    }

    if (fabs(residual_rpm_3) <= drpm_max) {
        current_rpm_3_ = target_rpm_3_;
    } else {
        current_rpm_3_ += sgn(residual_rpm_3) * drpm_max;
    }

    if (fabs(residual_rpm_4) <= drpm_max) {
        current_rpm_4_ = target_rpm_4_;
    } else {
        current_rpm_4_ += sgn(residual_rpm_4) * drpm_max;
    }

    block(
        jag_1_.speed_set(current_rpm_1_),
        jag_2_.speed_set(current_rpm_2_),
	    jag_3_.speed_set(current_rpm_3_),
        jag_4_.speed_set(current_rpm_4_)
    );
    ROS_INFO("crpm_1: %f, crpm_2: %f, crpm_3: %f, crpm_4: %f", current_rpm_1_, current_rpm_2_, current_rpm_3_, current_rpm_4_);
}

void AresDriveRobot::odom_set_circumference(double circum_m)
{
    wheel_circum_ = circum_m;
}

void AresDriveRobot::odom_set_separation(double separation_m)
{
    wheel_sep_ = separation_m;
}

void AresDriveRobot::odom_set_encoders(uint16_t cpr)
{
    block(
        jag_1_.config_encoders_set(cpr),
        jag_2_.config_encoders_set(cpr),
        jag_3_.config_encoders_set(cpr),
        jag_4_.config_encoders_set(cpr)
    );
}

void AresDriveRobot::odom_set_rate(uint8_t rate_ms)
{
    block(
        jag_1_.periodic_enable(0, rate_ms),
        jag_2_.periodic_enable(0, rate_ms),
        jag_3_.periodic_enable(0, rate_ms),
        jag_4_.periodic_enable(0, rate_ms)
    );
}

void AresDriveRobot::diag_set_rate(uint8_t rate_ms)
{
    block(
        jag_1_.periodic_enable(1, rate_ms),
        jag_2_.periodic_enable(1, rate_ms),
        jag_3_.periodic_enable(1, rate_ms),
        jag_4_.periodic_enable(1, rate_ms)
    );
}

void AresDriveRobot::heartbeat(void)
{
    jag_broadcast_.heartbeat();
}

/*
 * Wheel Odometry
 */

void AresDriveRobot::odom_attach(boost::function<OdometryCallback> callback)
{
    odom_signal_.connect(callback);
}

void AresDriveRobot::diag_attach(
    boost::function<DiagnosticsCallback> callback_one,
    boost::function<DiagnosticsCallback> callback_two,
    boost::function<DiagnosticsCallback> callback_three,
    boost::function<DiagnosticsCallback> callback_four)
{
    diag_1_signal_.connect(callback_one);
    diag_2_signal_.connect(callback_two);
    diag_3_signal_.connect(callback_three);
    diag_4_signal_.connect(callback_four);
}

void AresDriveRobot::odom_init(void)
{
    x_ = 0.0;
    y_ = 0.0;
    theta_ = 0.0;

    // Ignore the first odometry message to establish the reference point.:
    // TODO: This is wrong.
    odom_1_.wheel = kOne;
    odom_1_.init = false;
    odom_2_.wheel = kTwo;
    odom_2_.init = false;
    odom_3_.wheel = kThree;
    odom_3_.init = false;
    odom_4_.wheel = kFour;
    odom_4_.init = false;

    // Just in case. This shouldn't really matter.
    odom_1_.pos_prev = 0;
    odom_1_.pos_curr = 0;
    odom_2_.pos_prev = 0;
    odom_2_.pos_curr = 0;
    odom_3_.pos_prev = 0;
    odom_3_.pos_curr = 0;
    odom_4_.pos_prev = 0;
    odom_4_.pos_curr = 0;

    // Configure the Jaguars to use optical encoders. They are used as both a
    // speed reference for velocity control and position reference for
    // odometry. As such, they must be configured for position control even
    // though we are are using speed control mode.
    block(
        jag_1_.position_set_reference(PositionReference::kQuadratureEncoder),
        jag_2_.position_set_reference(PositionReference::kQuadratureEncoder),
        jag_3_.position_set_reference(PositionReference::kQuadratureEncoder),
        jag_4_.position_set_reference(PositionReference::kQuadratureEncoder)
    );

    block(
        jag_1_.periodic_config_odom(0,
            boost::bind(&AresDriveRobot::odom_update, this,
                boost::ref(odom_1_), _1, _2)),
        jag_2_.periodic_config_odom(0,
            boost::bind(&AresDriveRobot::odom_update, this,
                boost::ref(odom_2_), _1, _2)),
        jag_3_.periodic_config_odom(0,
            boost::bind(&AresDriveRobot::odom_update, this,
                boost::ref(odom_3_), _1, _2)),
        jag_4_.periodic_config_odom(0,
            boost::bind(&AresDriveRobot::odom_update, this,
                boost::ref(odom_4_), _1, _2))
    );
    ROS_INFO("DOES THIS HAPPEN?");
}


/*void DiffDriveRobot::estop_attach(boost::function<EStopCallback> callback)
{
    estop_signal_.connect(callback);
}*/

void AresDriveRobot::odom_update(Odometry &odom, double pos, double vel)
{
    if (wheel_circum_ == 0 || wheel_sep_ == 0) return;

    odom.pos_prev = odom.pos_curr;
    odom.pos_curr = pos;
    odom.vel = vel;

    // Skip the first sample from each wheel. This is necessary in case the
    // encoders came up in an unknown state.
    if (!odom.init) {
        odom.pos_prev = pos;
        odom.pos_curr = pos;
        odom.init = true;
        return;
    }

    // Update the state variables to indicate which odometry readings we
    // already have. Trigger a callback once we've received a pair of readings.
    if (odom_state_ == kNone) {
        odom_state_ = odom.wheel;
    } else if (odom.wheel != odom_state_) {
        // Compute the difference between the last two updates. Speed is
        // measured in RPMs, so all of these values are measured in
        // revolutions.
        double revs_1 = odom_1_.pos_curr - odom_1_.pos_prev;
        double revs_2 = odom_2_.pos_curr - odom_2_.pos_prev;
        double revs_3 = odom_3_.pos_curr - odom_3_.pos_prev;
        double revs_4 = odom_4_.pos_curr - odom_4_.pos_prev;
        std::swap(revs_1, revs_2);
        std::swap(revs_3, revs_4);

        // Convert from revolutions to meters.
        double const meters_1 = revs_1 * wheel_circum_;
        double const meters_2 = revs_2 * wheel_circum_;
        double const meters_3 = revs_3 * wheel_circum_;
        double const meters_4 = revs_4 * wheel_circum_;

        // Use the robot model to convert from wheel odometry to
        // two-dimensional motion.
        // TODO: Switch to a better odometry model.
        double const meters  = (meters_1 + meters_2) / 2;
        double const radians = (meters_1 - meters_2) / wheel_sep_;
        x_ += meters * cos(theta_ + radians / 2);
        y_ += meters * sin(theta_ + radians / 2);
        theta_ = angles::normalize_angle(theta_ + radians);

        // Estimate the robot's current velocity.
        double const v1 = odom_1_.vel * wheel_circum_ / 60;
        double const v2 = odom_2_.vel * wheel_circum_ / 60;
        double const v3 = odom_3_.vel * wheel_circum_ / 60;
        double const v4 = odom_4_.vel * wheel_circum_ / 60;
        double const v_linear = (v1 + v2) / 2;
        double const omega    = (v2 - v1) / wheel_sep_;

        odom_signal_(x_, y_, theta_, v_linear, omega, v1, v2, v3, v4);
        odom_state_ = kNone;
    } else {
        std::cerr << "war: periodic update message was dropped" << std::endl;
    }
}

/*
 * Diagnostics
 */
void AresDriveRobot::diag_init(void)
{
    block(
        jag_1_.periodic_config_diag(1,
            boost::bind(&AresDriveRobot::diag_update, this,
                kOne, boost::ref(diag_1_), _1, _2, _3, _4)
        ),
        jag_2_.periodic_config_diag(1,
            boost::bind(&AresDriveRobot::diag_update, this,
                kTwo, boost::ref(diag_2_), _1, _2, _3, _4)
        ),
        jag_3_.periodic_config_diag(1,
            boost::bind(&AresDriveRobot::diag_update, this,
                kThree, boost::ref(diag_3_), _1, _2, _3, _4)
        ),
        jag_4_.periodic_config_diag(1,
            boost::bind(&AresDriveRobot::diag_update, this,
                kFour, boost::ref(diag_4_), _1, _2, _3, _4)
        )
    );

    // TODO: Make this a parameter.
    block(
        jag_1_.periodic_enable(1, 500),
        jag_2_.periodic_enable(1, 500),
        jag_3_.periodic_enable(1, 500),
        jag_4_.periodic_enable(1, 500)
    );
}

void AresDriveRobot::diag_update(
    Wheel wheel, Diagnostics &diag,
    LimitStatus::Enum limits, Fault::Enum faults,
    double voltage, double temperature)
{
    //bool const estop_before = diag_1_.stopped || diag_2_.stopped;

    // TODO: Check for a fault.
    diag.stopped = !(limits & 0x03);
    diag.voltage = voltage;
    diag.temperature = temperature;

    //bool const estop_after = diag_left_.stopped || diag_right_.stopped;

    // Only trigger an e-stop callback if the state changed. We don't know the
    // initial state, so the first update always triggers a callback.
    /*if (!diag_init_ || estop_after != estop_before) {
        estop_signal_(estop_after);
    }
    diag_init_ = true;
*/

    // Other diagnostics (i.e. bus voltage and temperature) use separate left
    // and right callbacks.
    if (wheel == kOne) {
        diag_1_signal_(voltage, temperature);
    } else if (wheel == kTwo) {
        diag_2_signal_(voltage, temperature);
    } else if (wheel == kThree) {
        diag_3_signal_(voltage, temperature);
    } else if (wheel == kFour) {
        diag_4_signal_(voltage, temperature);
    }
}

/*
 * Speed Control
 */
void AresDriveRobot::speed_set_p(double p)
{
    block(
        jag_1_.speed_set_p(flip_1_ * p),
        jag_2_.speed_set_p(flip_2_ * p),
        jag_3_.speed_set_p(flip_3_ * p),
        jag_4_.speed_set_p(flip_4_ * p)
    );
}

void AresDriveRobot::speed_set_i(double i)
{
    block(
        jag_1_.speed_set_i(flip_1_ * i),
        jag_2_.speed_set_i(flip_2_ * i),
        jag_3_.speed_set_i(flip_3_ * i),
        jag_4_.speed_set_i(flip_4_ * i)
    );
}

void AresDriveRobot::speed_set_d(double d)
{
    block(
        jag_1_.speed_set_d(flip_1_ * d),
        jag_2_.speed_set_d(flip_2_ * d),
        jag_3_.speed_set_d(flip_3_ * d),
        jag_4_.speed_set_d(flip_4_ * d)
    );
}

void AresDriveRobot::speed_init(void)
{
    block(
        jag_1_.speed_set_reference(SpeedReference::kQuadratureEncoder),
        jag_2_.speed_set_reference(SpeedReference::kQuadratureEncoder),
        jag_3_.speed_set_reference(SpeedReference::kQuadratureEncoder),
        jag_4_.speed_set_reference(SpeedReference::kQuadratureEncoder)
    );
    block(
        jag_1_.speed_enable(),
        jag_2_.speed_enable(),
        jag_3_.speed_enable(),
        jag_4_.speed_enable()
    );
    current_rpm_1_ = 0;
    current_rpm_2_ = 0;
    current_rpm_3_ = 0;
    current_rpm_4_ = 0;

    target_rpm_1_ = 0;
    target_rpm_2_ = 0;
    target_rpm_3_ = 0;
    target_rpm_4_ = 0;
}

/*
 * Helper Methods
 */
void AresDriveRobot::block(can::TokenPtr t1, can::TokenPtr t2, can::TokenPtr t3, can::TokenPtr t4)
{
    t1->block();
    t2->block();
    t3->block();
    t4->block();
}

};
